package novemberBot;

import novemberBot.communication.ConnectionManager;
import novemberBot.mapmaking.Map;
import novemberBot.navigation.*;
import novemberBot.sensor.*;
import novemberBot.utils.Debugger;
import lejos.nxt.Motor;
import lejos.nxt.MotorPort;
import lejos.nxt.SensorPort;
import lejos.nxt.comm.NXTConnection;
import lejos.nxt.comm.RS485;

/**Singleton representing the entire robot*/
public class FactoryFloorRobot extends TwoWheeledRobot {
	/**Reference to the only allowed instance of the singleton*/
	private static FactoryFloorRobot INSTANCE;
	
	/**drop of Coords*/
	public static final double[] DROP_OFF = new double[]{10*30.48, 10*30.48};
	
	/**reference to connection manager*/
	private static ConnectionManager cm;
	/**load the bot is carrying*/
	private int load=0;
	private static final int MAX_LOAD=6;
	/**map from high sensor*/
	private Map highMap;
	/**map from low sensor*/
	private Map lowMap;
	/**Odometer calculates position and heading*/
	private Odometer odo;
	/**Navigator drives the FactoryFloorRobot around*/
	private Navigator theCaptain;	
	/**UltraSonic Polling Class*/
	private UsPoller usp;
	/**reference to Arm remote, controls Arm object on other brick*/
	private ArmRemote arm;
	/**Line checkers*/
	private LineChecker Left; 
	/**Line checkers*/
	private LineChecker Right;
	
	/**Private constructor for singleton implementation */
	public FactoryFloorRobot(boolean connect) {
		super(Motor.B, Motor.A);
		if(INSTANCE!=null){
			Debugger.fail();
			return;
		}
		if(connect){
			boolean connected =false;
			NXTConnection nxtc;
			while(!connected){
				nxtc= RS485.connect("001653042A26");
				try{	
					cm = new ConnectionManager(nxtc, true);
					connected=true;
				}catch(Exception e){}
			}
		}
		INSTANCE=this;
	}
	/**Initialises the class objects*/
	public void initialize(){
		odo=new Odometer(this);
		usp=new UsPoller(MotorPort.C, SensorPort.S2, SensorPort.S3);
		
		if (cm != null)
			arm = new ArmRemote(cm);
		highMap=new Map(UsDataResponder.Sensor.HIGH, 13);
		lowMap=new Map(UsDataResponder.Sensor.LOW, 13);	
		usp.addResponder(lowMap);
		usp.addResponder(highMap);
		theCaptain=new Navigator();
		Left = new LeftLineChecker(SensorPort.S1); 
		if (cm != null)
			Right = new RightLineChecker();
	}
	
	public void startMapMaking(){
		Thread hm = new Thread(highMap);
		hm.start();
		Thread lm = new Thread(lowMap);
		lm.start();
	}
	
	/**returns the instance of this singleton, creates it if not done yet*/
	public static FactoryFloorRobot getInstance(){
		return INSTANCE;
	}

	public boolean spaceLeft(){
		return load<MAX_LOAD;
	}
	
	public boolean isMoving() {
		return(rightMotor.isMoving() || leftMotor.isMoving() /*|| theCaptain.isNavigating()*/);
		//return(forwardSpeed!=0 || rotationSpeed!=0);
	}
	
	//mutators
	/**mutator for load*/
	public void setLoad(int load){
		this.load=load;
	}
	
	
	//accessors
	/**accessor for load*/
	public int getLoad(){
		return load;
	}
	
	/**accessor for Odometer*/
	public Odometer getOdometer(){
		return odo;
	}
	
	/**accessor for Navigator*/
	public Navigator getNavigator(){
		return theCaptain;
	}
	
	/**accessor for UsPoller*/
	public UsPoller getUsPoller(){
		return usp;
	}
	/**accessor for ArmRemote*/
	public ArmRemote getArmRemote() {
		return arm;
	}
	/**accessor for ConnectionManager*/
	public ConnectionManager getConnectionManager() {
		while(cm==null)Thread.yield();
		return cm;
	}
	/**accessor for highMap*/
	public Map getHighMap() {
		return highMap;
	}
	
	/**accessor for lowMap*/
	public Map getLowMap() {
		return lowMap;
	}
	public LineChecker getLeftChecker(){
		return Left;
	}
	public LineChecker getRightChecker(){
		return Right;
	}
	
}
